
# include "dfxMatrix4.h"

namespace DFX
{
    Matrix4::Matrix4()
    {
        std::memset(M2, 0, sizeof(Float32)* 16);

        M2[0][0] = M2[1][1] = 1.0f;
        M2[2][2] = M2[3][3] = 1.0f;
    }

    Matrix4::Matrix4(const Matrix4 &m)
    {
        std::memcpy(M2, m.M2, sizeof(Float32)* 16);
    }

    void Matrix4::SetIdentity()
    {
        std::memset(M2, 0, sizeof(Float32)* 16);

        M2[0][0] = M2[1][1] = 1.0f;
        M2[2][2] = M2[3][3] = 1.0f;
    }

    Float32 Matrix4::GetDeterminant() const
    {
        return	M2[0][0] * (
            M2[1][1] * (M2[2][2] * M2[3][3] - M2[2][3] * M2[3][2]) -
            M2[2][1] * (M2[1][2] * M2[3][3] - M2[1][3] * M2[3][2]) +
            M2[3][1] * (M2[1][2] * M2[2][3] - M2[1][3] * M2[2][2])
            ) -
            M2[1][0] * (
            M2[0][1] * (M2[2][2] * M2[3][3] - M2[2][3] * M2[3][2]) -
            M2[2][1] * (M2[0][2] * M2[3][3] - M2[0][3] * M2[3][2]) +
            M2[3][1] * (M2[0][2] * M2[2][3] - M2[0][3] * M2[2][2])
            ) +
            M2[2][0] * (
            M2[0][1] * (M2[1][2] * M2[3][3] - M2[1][3] * M2[3][2]) -
            M2[1][1] * (M2[0][2] * M2[3][3] - M2[0][3] * M2[3][2]) +
            M2[3][1] * (M2[0][2] * M2[1][3] - M2[0][3] * M2[1][2])
            ) -
            M2[3][0] * (
            M2[0][1] * (M2[1][2] * M2[2][3] - M2[1][3] * M2[2][2]) -
            M2[1][1] * (M2[0][2] * M2[2][3] - M2[0][3] * M2[2][2]) +
            M2[2][1] * (M2[0][2] * M2[1][3] - M2[0][3] * M2[1][2])
            );
    }

    Matrix4 Matrix4::GetInverse() const
    {
        Matrix4 out;
        Float32 det = GetDeterminant();

        if (det == 0.0f)
            return out;

        Float32 invDet = 1.0f / det;

        out.M2[0][0] = invDet * (
            M2[1][1] * (M2[2][2] * M2[3][3] - M2[2][3] * M2[3][2]) -
            M2[2][1] * (M2[1][2] * M2[3][3] - M2[1][3] * M2[3][2]) +
            M2[3][1] * (M2[1][2] * M2[2][3] - M2[1][3] * M2[2][2])
            );
        out.M2[0][1] = -invDet * (
            M2[0][1] * (M2[2][2] * M2[3][3] - M2[2][3] * M2[3][2]) -
            M2[2][1] * (M2[0][2] * M2[3][3] - M2[0][3] * M2[3][2]) +
            M2[3][1] * (M2[0][2] * M2[2][3] - M2[0][3] * M2[2][2])
            );
        out.M2[0][2] = invDet * (
            M2[0][1] * (M2[1][2] * M2[3][3] - M2[1][3] * M2[3][2]) -
            M2[1][1] * (M2[0][2] * M2[3][3] - M2[0][3] * M2[3][2]) +
            M2[3][1] * (M2[0][2] * M2[1][3] - M2[0][3] * M2[1][2])
            );
        out.M2[0][3] = -invDet * (
            M2[0][1] * (M2[1][2] * M2[2][3] - M2[1][3] * M2[2][2]) -
            M2[1][1] * (M2[0][2] * M2[2][3] - M2[0][3] * M2[2][2]) +
            M2[2][1] * (M2[0][2] * M2[1][3] - M2[0][3] * M2[1][2])
            );

        out.M2[1][0] = -invDet * (
            M2[1][0] * (M2[2][2] * M2[3][3] - M2[2][3] * M2[3][2]) -
            M2[2][0] * (M2[1][2] * M2[3][3] - M2[1][3] * M2[3][2]) +
            M2[3][0] * (M2[1][2] * M2[2][3] - M2[1][3] * M2[2][2])
            );
        out.M2[1][1] = invDet * (
            M2[0][0] * (M2[2][2] * M2[3][3] - M2[2][3] * M2[3][2]) -
            M2[2][0] * (M2[0][2] * M2[3][3] - M2[0][3] * M2[3][2]) +
            M2[3][0] * (M2[0][2] * M2[2][3] - M2[0][3] * M2[2][2])
            );
        out.M2[1][2] = -invDet * (
            M2[0][0] * (M2[1][2] * M2[3][3] - M2[1][3] * M2[3][2]) -
            M2[1][0] * (M2[0][2] * M2[3][3] - M2[0][3] * M2[3][2]) +
            M2[3][0] * (M2[0][2] * M2[1][3] - M2[0][3] * M2[1][2])
            );
        out.M2[1][3] = invDet * (
            M2[0][0] * (M2[1][2] * M2[2][3] - M2[1][3] * M2[2][2]) -
            M2[1][0] * (M2[0][2] * M2[2][3] - M2[0][3] * M2[2][2]) +
            M2[2][0] * (M2[0][2] * M2[1][3] - M2[0][3] * M2[1][2])
            );

        out.M2[2][0] = invDet * (
            M2[1][0] * (M2[2][1] * M2[3][3] - M2[2][3] * M2[3][1]) -
            M2[2][0] * (M2[1][1] * M2[3][3] - M2[1][3] * M2[3][1]) +
            M2[3][0] * (M2[1][1] * M2[2][3] - M2[1][3] * M2[2][1])
            );
        out.M2[2][1] = -invDet * (
            M2[0][0] * (M2[2][1] * M2[3][3] - M2[2][3] * M2[3][1]) -
            M2[2][0] * (M2[0][1] * M2[3][3] - M2[0][3] * M2[3][1]) +
            M2[3][0] * (M2[0][1] * M2[2][3] - M2[0][3] * M2[2][1])
            );
        out.M2[2][2] = invDet * (
            M2[0][0] * (M2[1][1] * M2[3][3] - M2[1][3] * M2[3][1]) -
            M2[1][0] * (M2[0][1] * M2[3][3] - M2[0][3] * M2[3][1]) +
            M2[3][0] * (M2[0][1] * M2[1][3] - M2[0][3] * M2[1][1])
            );
        out.M2[2][3] = -invDet * (
            M2[0][0] * (M2[1][1] * M2[2][3] - M2[1][3] * M2[2][1]) -
            M2[1][0] * (M2[0][1] * M2[2][3] - M2[0][3] * M2[2][1]) +
            M2[2][0] * (M2[0][1] * M2[1][3] - M2[0][3] * M2[1][1])
            );

        out.M2[3][0] = -invDet * (
            M2[1][0] * (M2[2][1] * M2[3][2] - M2[2][2] * M2[3][1]) -
            M2[2][0] * (M2[1][1] * M2[3][2] - M2[1][2] * M2[3][1]) +
            M2[3][0] * (M2[1][1] * M2[2][2] - M2[1][2] * M2[2][1])
            );
        out.M2[3][1] = invDet * (
            M2[0][0] * (M2[2][1] * M2[3][2] - M2[2][2] * M2[3][1]) -
            M2[2][0] * (M2[0][1] * M2[3][2] - M2[0][2] * M2[3][1]) +
            M2[3][0] * (M2[0][1] * M2[2][2] - M2[0][2] * M2[2][1])
            );
        out.M2[3][2] = -invDet * (
            M2[0][0] * (M2[1][1] * M2[3][2] - M2[1][2] * M2[3][1]) -
            M2[1][0] * (M2[0][1] * M2[3][2] - M2[0][2] * M2[3][1]) +
            M2[3][0] * (M2[0][1] * M2[1][2] - M2[0][2] * M2[1][1])
            );
        out.M2[3][3] = invDet * (
            M2[0][0] * (M2[1][1] * M2[2][2] - M2[1][2] * M2[2][1]) -
            M2[1][0] * (M2[0][1] * M2[2][2] - M2[0][2] * M2[2][1]) +
            M2[2][0] * (M2[0][1] * M2[1][2] - M2[0][2] * M2[1][1])
            );

        return out;
    }

    Matrix4 Matrix4::GetTranspose() const
    {
        Matrix4 out;

        out.M2[0][0] = M2[0][0];
        out.M2[0][1] = M2[1][0];
        out.M2[0][2] = M2[2][0];
        out.M2[0][3] = M2[3][0];

        out.M2[1][0] = M2[0][1];
        out.M2[1][1] = M2[1][1];
        out.M2[1][2] = M2[2][1];
        out.M2[1][3] = M2[3][1];

        out.M2[2][0] = M2[0][2];
        out.M2[2][1] = M2[1][2];
        out.M2[2][2] = M2[2][2];
        out.M2[2][3] = M2[3][2];

        out.M2[3][0] = M2[0][3];
        out.M2[3][1] = M2[1][3];
        out.M2[3][2] = M2[2][3];
        out.M2[3][3] = M2[3][3];

        return out;
    }

    Vector3 Matrix4::TransformVector3(const Vector3 &v) const
    {
        Vector3 out;

        out.X = v.X * M2[0][0] + v.Y * M2[1][0] + v.Z * M2[2][0] + M2[3][0];
        out.Y = v.X * M2[0][1] + v.Y * M2[1][1] + v.Z * M2[2][1] + M2[3][1];
        out.Z = v.X * M2[0][2] + v.Y * M2[1][2] + v.Z * M2[2][2] + M2[3][2];

        return out;
    }

    Vector3 Matrix4::InvTransformVector3(const Vector3 &v) const
    {
        Vector3 t, out;

        t.X = v.X - M2[3][0];
        t.Y = v.Y - M2[3][1];
        t.Z = v.Z - M2[3][2];

        out.X = t.X * M2[0][0] + t.Y * M2[1][0] + t.Z * M2[2][0];
        out.Y = t.X * M2[0][1] + t.Y * M2[1][1] + t.Z * M2[2][1];
        out.Z = t.X * M2[0][2] + t.Y * M2[1][2] + t.Z * M2[2][2];

        return out;
    }

    Vector3 Matrix4::TransformNormalVector3(const Vector3 &v) const
    {
        Vector3 out;

        out.X = v.X * M2[0][0] + v.Y * M2[1][0] + v.Z * M2[2][0];
        out.Y = v.X * M2[0][1] + v.Y * M2[1][1] + v.Z * M2[2][1];
        out.Z = v.X * M2[0][2] + v.Y * M2[1][2] + v.Z * M2[2][2];

        return out;
    }

    Vector4 Matrix4::TransformVector4(const Vector4 &v) const
    {
        Vector4 out;

        out.X = v.X * M2[0][0] + v.Y * M2[1][0] + v.Z * M2[2][0] + v.W * M2[3][0];
        out.Y = v.X * M2[0][1] + v.Y * M2[1][1] + v.Z * M2[2][1] + v.W * M2[3][1];
        out.Z = v.X * M2[0][2] + v.Y * M2[1][2] + v.Z * M2[2][2] + v.W * M2[3][2];

        return out;
    }

    Vector4 Matrix4::InvTransformVector4(const Vector4 &v) const
    {
        Vector4 t, out;

        t.X = v.X - M2[3][0];
        t.Y = v.Y - M2[3][1];
        t.Z = v.Z - M2[3][2];
        t.W = v.W - M2[3][3];

        out.X = t.X * M2[0][0] + t.Y * M2[1][0] + t.Z * M2[2][0] + t.W * M2[3][0];
        out.Y = t.X * M2[0][1] + t.Y * M2[1][1] + t.Z * M2[2][1] + t.W * M2[3][1];
        out.Z = t.X * M2[0][2] + t.Y * M2[1][2] + t.Z * M2[2][2] + t.W * M2[3][2];

        return out;
    }

    Matrix4 Matrix4::operator * (const Matrix4 &m) const
    {
        Matrix4 out;

        out.M2[0][0] = M2[0][0] * m.M2[0][0] + M2[0][1] * m.M2[1][0] + M2[0][2] * m.M2[2][0] + M2[0][3] * m.M2[3][0];
        out.M2[0][1] = M2[0][0] * m.M2[0][1] + M2[0][1] * m.M2[1][1] + M2[0][2] * m.M2[2][1] + M2[0][3] * m.M2[3][1];
        out.M2[0][2] = M2[0][0] * m.M2[0][2] + M2[0][1] * m.M2[1][2] + M2[0][2] * m.M2[2][2] + M2[0][3] * m.M2[3][2];
        out.M2[0][3] = M2[0][0] * m.M2[0][3] + M2[0][1] * m.M2[1][3] + M2[0][2] * m.M2[2][3] + M2[0][3] * m.M2[3][3];

        out.M2[1][0] = M2[1][0] * m.M2[0][0] + M2[1][1] * m.M2[1][0] + M2[1][2] * m.M2[2][0] + M2[1][3] * m.M2[3][0];
        out.M2[1][1] = M2[1][0] * m.M2[0][1] + M2[1][1] * m.M2[1][1] + M2[1][2] * m.M2[2][1] + M2[1][3] * m.M2[3][1];
        out.M2[1][2] = M2[1][0] * m.M2[0][2] + M2[1][1] * m.M2[1][2] + M2[1][2] * m.M2[2][2] + M2[1][3] * m.M2[3][2];
        out.M2[1][3] = M2[1][0] * m.M2[0][3] + M2[1][1] * m.M2[1][3] + M2[1][2] * m.M2[2][3] + M2[1][3] * m.M2[3][3];

        out.M2[2][0] = M2[2][0] * m.M2[0][0] + M2[2][1] * m.M2[1][0] + M2[2][2] * m.M2[2][0] + M2[2][3] * m.M2[3][0];
        out.M2[2][1] = M2[2][0] * m.M2[0][1] + M2[2][1] * m.M2[1][1] + M2[2][2] * m.M2[2][1] + M2[2][3] * m.M2[3][1];
        out.M2[2][2] = M2[2][0] * m.M2[0][2] + M2[2][1] * m.M2[1][2] + M2[2][2] * m.M2[2][2] + M2[2][3] * m.M2[3][2];
        out.M2[2][3] = M2[2][0] * m.M2[0][3] + M2[2][1] * m.M2[1][3] + M2[2][2] * m.M2[2][3] + M2[2][3] * m.M2[3][3];

        out.M2[3][0] = M2[3][0] * m.M2[0][0] + M2[3][1] * m.M2[1][0] + M2[3][2] * m.M2[2][0] + M2[3][3] * m.M2[3][0];
        out.M2[3][1] = M2[3][0] * m.M2[0][1] + M2[3][1] * m.M2[1][1] + M2[3][2] * m.M2[2][1] + M2[3][3] * m.M2[3][1];
        out.M2[3][2] = M2[3][0] * m.M2[0][2] + M2[3][1] * m.M2[1][2] + M2[3][2] * m.M2[2][2] + M2[3][3] * m.M2[3][2];
        out.M2[3][3] = M2[3][0] * m.M2[0][3] + M2[3][1] * m.M2[1][3] + M2[3][2] * m.M2[2][3] + M2[3][3] * m.M2[3][3];

        return out;
    }

    Matrix4& Matrix4::operator *= (const Matrix4 &m)
    {
        Matrix4 out;

        out.M2[0][0] = M2[0][0] * m.M2[0][0] + M2[0][1] * m.M2[1][0] + M2[0][2] * m.M2[2][0] + M2[0][3] * m.M2[3][0];
        out.M2[0][1] = M2[0][0] * m.M2[0][1] + M2[0][1] * m.M2[1][1] + M2[0][2] * m.M2[2][1] + M2[0][3] * m.M2[3][1];
        out.M2[0][2] = M2[0][0] * m.M2[0][2] + M2[0][1] * m.M2[1][2] + M2[0][2] * m.M2[2][2] + M2[0][3] * m.M2[3][2];
        out.M2[0][3] = M2[0][0] * m.M2[0][3] + M2[0][1] * m.M2[1][3] + M2[0][2] * m.M2[2][3] + M2[0][3] * m.M2[3][3];

        out.M2[1][0] = M2[1][0] * m.M2[0][0] + M2[1][1] * m.M2[1][0] + M2[1][2] * m.M2[2][0] + M2[1][3] * m.M2[3][0];
        out.M2[1][1] = M2[1][0] * m.M2[0][1] + M2[1][1] * m.M2[1][1] + M2[1][2] * m.M2[2][1] + M2[1][3] * m.M2[3][1];
        out.M2[1][2] = M2[1][0] * m.M2[0][2] + M2[1][1] * m.M2[1][2] + M2[1][2] * m.M2[2][2] + M2[1][3] * m.M2[3][2];
        out.M2[1][3] = M2[1][0] * m.M2[0][3] + M2[1][1] * m.M2[1][3] + M2[1][2] * m.M2[2][3] + M2[1][3] * m.M2[3][3];

        out.M2[2][0] = M2[2][0] * m.M2[0][0] + M2[2][1] * m.M2[1][0] + M2[2][2] * m.M2[2][0] + M2[2][3] * m.M2[3][0];
        out.M2[2][1] = M2[2][0] * m.M2[0][1] + M2[2][1] * m.M2[1][1] + M2[2][2] * m.M2[2][1] + M2[2][3] * m.M2[3][1];
        out.M2[2][2] = M2[2][0] * m.M2[0][2] + M2[2][1] * m.M2[1][2] + M2[2][2] * m.M2[2][2] + M2[2][3] * m.M2[3][2];
        out.M2[2][3] = M2[2][0] * m.M2[0][3] + M2[2][1] * m.M2[1][3] + M2[2][2] * m.M2[2][3] + M2[2][3] * m.M2[3][3];

        out.M2[3][0] = M2[3][0] * m.M2[0][0] + M2[3][1] * m.M2[1][0] + M2[3][2] * m.M2[2][0] + M2[3][3] * m.M2[3][0];
        out.M2[3][1] = M2[3][0] * m.M2[0][1] + M2[3][1] * m.M2[1][1] + M2[3][2] * m.M2[2][1] + M2[3][3] * m.M2[3][1];
        out.M2[3][2] = M2[3][0] * m.M2[0][2] + M2[3][1] * m.M2[1][2] + M2[3][2] * m.M2[2][2] + M2[3][3] * m.M2[3][2];
        out.M2[3][3] = M2[3][0] * m.M2[0][3] + M2[3][1] * m.M2[1][3] + M2[3][2] * m.M2[2][3] + M2[3][3] * m.M2[3][3];

        *this = out;

        return *this;
    }

    Matrix4 Matrix4::Translate(const Vector3 &v)
    {
        Matrix4 out(*this);

        out.M2[3][0] = M2[0][0] * v.X + M2[1][0] * v.Y + M2[2][0] * v.Z + M2[3][0];
        out.M2[3][1] = M2[0][1] * v.X + M2[1][1] * v.Y + M2[2][1] * v.Z + M2[3][1];
        out.M2[3][2] = M2[0][2] * v.X + M2[1][2] * v.Y + M2[2][2] * v.Z + M2[3][2];
        out.M2[3][3] = M2[0][3] * v.X + M2[1][3] * v.Y + M2[2][3] * v.Z + M2[3][3];

        return out;
    }

    Matrix4 Matrix4::Scale(const Vector3 &v)
    {
        Matrix4 out(*this);

        out.M2[0][0] = M2[0][0] * v.X;
        out.M2[0][1] = M2[0][1] * v.X;
        out.M2[0][2] = M2[0][2] * v.X;
        out.M2[0][3] = M2[0][3] * v.X;

        out.M2[1][0] = M2[1][0] * v.Y;
        out.M2[1][1] = M2[1][1] * v.Y;
        out.M2[1][2] = M2[1][2] * v.Y;
        out.M2[1][3] = M2[1][3] * v.Y;

        out.M2[2][0] = M2[2][0] * v.Z;
        out.M2[2][1] = M2[2][1] * v.Z;
        out.M2[2][2] = M2[2][2] * v.Z;
        out.M2[2][3] = M2[2][3] * v.Z;

        return out;
    }

    Matrix4 Matrix4::Rotate(const Vector3 &v, Float32 a)
    {
        Float32 s = std::sinf(DFX_TORADIANS(a)),
            c = std::cosf(DFX_TORADIANS(a));

        Vector3 t = v;
        t.Normalize();

        Float32 xx, yy, zz, xy, yz, zx,
            xs, ys, zs, c1;

        Matrix4 m;

        xx = t.X * t.X;
        yy = t.Y * t.Y;
        zz = t.Z * t.Z;
        xy = t.X * t.Y;
        yz = t.Y * t.Z;
        zx = t.Z * t.X;
        xs = t.X * s;
        ys = t.Y * s;
        zs = t.Z * s;
        c1 = 1.0f - c;

        m.M2[0][0] = (c1 * xx) + c;
        m.M2[1][0] = (c1 * xy) - zs;
        m.M2[2][0] = (c1 * zx) + ys;

        m.M2[0][1] = (c1 * xy) + zs;
        m.M2[1][1] = (c1 * yy) + c;
        m.M2[2][1] = (c1 * yz) - xs;

        m.M2[0][2] = (c1 * zx) - ys;
        m.M2[1][2] = (c1 * yz) + xs;
        m.M2[2][2] = (c1 * zz) + c;

        return *this * m;
    }

    Matrix4 Matrix4::CreatePerspective(Float32 fov, Float32 aspect, Float32 nearZ, Float32 farZ)
    {
        Matrix4 out;

        Float32 ys = cosf(DFX_TORADIANS(fov * 0.5f)) / sinf(DFX_TORADIANS(fov * 0.5f));
        Float32 xs = ys / aspect;
        Float32 ds = farZ - nearZ;

        out.M2[0][0] = xs;
        out.M2[0][1] = 0.0f;
        out.M2[0][2] = 0.0f;
        out.M2[0][3] = 0.0f;
        out.M2[1][0] = 0.0f;
        out.M2[1][1] = ys;
        out.M2[1][2] = 0.0f;
        out.M2[1][3] = 0.0f;
        out.M2[2][0] = 0.0f;
        out.M2[2][1] = 0.0f;
        out.M2[2][2] = -(farZ + nearZ) / ds;
        out.M2[2][3] = -1.0f;
        out.M2[3][0] = 0.0f;
        out.M2[3][1] = 0.0f;
        out.M2[3][2] = -2.0f * (nearZ * farZ) / ds;
        out.M2[3][3] = 0.0f;

        return out;
    }

    Matrix4 Matrix4::CreateOrthogonal(Float32 left, Float32 top, Float32 right, Float32 bottom, Float32 nearZ, Float32 farZ)
    {
        Matrix4 out;

        out.M2[0][0] = 2.0f / (right - left);
        out.M2[0][1] = 0.0f;
        out.M2[0][2] = 0.0f;
        out.M2[0][3] = 0.0f;
        out.M2[1][0] = 0.0f;
        out.M2[1][1] = 2.0f / (top - bottom);
        out.M2[1][2] = 0.0f;
        out.M2[1][3] = 0.0f;
        out.M2[2][0] = 0.0f;
        out.M2[2][1] = 0.0f;
        out.M2[2][2] = -2.0f / (farZ - nearZ);
        out.M2[2][3] = 0.0f;
        out.M2[3][0] = -(right + left) / (right - left);
        out.M2[3][1] = -(top + bottom) / (top - bottom);
        out.M2[3][2] = -(farZ + nearZ) / (farZ - nearZ);
        out.M2[3][3] = 1.0f;

        return out;
    }

    Matrix4 Matrix4::CreateLookAt(const Vector3 &eye, const Vector3 &target, const Vector3 &up)
    {
        Matrix4 out;
        Vector3 f, s, u;

        f = target - eye;
        f.Normalize();

        s = f.Cross(up);
        s.Normalize();

        u = s.Cross(f);

        out.M2[0][0] = s.X;
        out.M2[1][0] = s.Y;
        out.M2[2][0] = s.Z;

        out.M2[0][1] = u.X;
        out.M2[1][1] = u.Y;
        out.M2[2][1] = u.Z;

        out.M2[0][2] = -f.X;
        out.M2[1][2] = -f.Y;
        out.M2[2][2] = -f.Z;

        out.M2[3][0] = -s.Dot(eye);
        out.M2[3][1] = -u.Dot(eye);
        out.M2[3][2] = -f.Dot(eye);

        return out;
    }
}